/*
 * @Author: YuanQuan D
 * @Date: 2025-04-01 20:13:56
 * @LastEditTime: 2025-04-01 23:10:50
 * @FilePath: \F429_arm_control_slave\User_Code\Inc\can_cfg.h
 */
#ifndef _CAN_CFG_H_
#define _CAN_CFG_H_

#include "main.h"
#include "cmsis_os.h"

#define RMD_Read_PID_Command 0X30
#define RMD_Write_PID_RAM_Command 0x31
#define RMD_Write_PID_ROM_Command 0x32
#define RMD_Read_Acc_Command 0x33
#define RMD_Write_Acc_RAM_Command 0x34
#define RMD_Read_Encoder_Command 0x90
#define RMD_Write_Encoder_ROM_Calibrate_Zero_Command 0x91
#define RMD_Write_Pos_ROM_Calibrate_Zero_Command 0x19
#define RMD_Read_Stack_Angle_Command 0x92
#define RMD_Read_Single_Angle_Command 0x94
#define RMD_Set_Motor_Inital_Pos_Command 0x95
#define RMD_Stop_Motor_Command 0x80
#define RMD_Pause_Motor_Command 0x81
#define RMD_Run_Motor_Command 0x88
#define RMD_Torque_Closed_Loop_Control_Command 0xA1 
#define RMD_Speed_Closed_Loop_Control_Command 0xA2
#define RMD_Pos_Closed_Loop_Control_Command_1 0xA3
#define RMD_Pos_Closed_Loop_Control_Command_2 0xA4
#define RMD_Pos_Closed_Loop_Control_Command_3 0xA5
#define RMD_Pos_Closed_Loop_Control_Command_4 0xA6
#define RMD_Pos_Closed_Loop_Control_Command_5 0xA7
#define RMD_Pos_Closed_Loop_Control_Command_6 0xA8

#define std_id 0x140
#define RMD_CAN_DLC 8
#define Joint_CAN_DLC_5 5
#define Joint_CAN_DLC_1 1

#define Joint_Set_Pos_Model_Pos_Angle_Command 0x1E
#define Joint_Set_Max_Positive_Pos_Command 0x26
#define Joint_Set_Min_Negative_Pos_Command 0x27
#define Joint_Get_Now_Pos_Command 0x08
#define Joint_1_Angle_Max_Limit 3300000
#define Joint_1_Angle_Min_Limit -3300000
#define Joint_2_Set_Max_Positive_Pos -100000
#define Joint_2_Set_Min_Negative_Pos -3000000
#define Joint_2_Angle_Max_Limit -100000
#define Joint_2_Angle_Min_Limit -3100000

#define Joint_Clear_Err_Command 0X0A

#define Reset_Motor 0xB0
#define Forward_90_Command 0xB1
#define Turn_Right_90_Command 0xB2
#define Inclined_Top_90_Command 0xB3
// #define Wave_Hand 0xB4
#define Fixed_trajectory_cruise_Command 0xB4
#define Draw_Triangular 0xB5
#define Draw_circle 0xB6


#define Motor_Ctr_host_Command 0x99


void can_filter(void);
void RMD_TorgueControl(uint8_t Motor_ID, int32_t iqControl);
void RMD_speedControl(uint8_t Motor_ID, int32_t speedControl);
void RMD_Multi_angleControl_1(uint8_t Motor_ID, int32_t angleControl);
void RMD_Multi_angleControl_2(uint8_t Motor_ID, uint16_t maxSpeed, int32_t angleControl);
void RMD_Single_loop_angleControl_1(uint8_t Motor_ID, uint8_t spinDirection, uint16_t angleControl);
void RMD_Single_loop_angleControl_2(uint8_t Motor_ID, uint8_t spinDirection, uint16_t maxSpeed, uint16_t angleControl);
void RMD_Motor_Off(uint8_t Motor_ID);
void RMD_Motor_Normal_Ctr(uint8_t Motor_ID, uint8_t cmd);
void Joint_CAN_SendPosition(uint8_t Motor_ID, int32_t angleControl);
void Joint_CAN_Set_Pos_Limit(uint8_t Motor_ID, int32_t Limit_P_Pos, int32_t Limit_N_Pos);
void Joint_CAN_Single_Command_Ctr(uint8_t Motor_ID, uint8_t cmd);
void RMD_Mul_abs_loop_angleControl(uint8_t Motor_ID, uint16_t maxSpeed, uint32_t angleControl);
int32_t map_intoin(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max);
float map_intoflo(int32_t x, int32_t in_min, int32_t in_max, float out_min, float out_max);  
int32_t map_float_to_int(double x, double in_min, double in_max, int32_t out_min, int32_t out_max);
void canCallback_datadeal();

#endif